class rocketOSFC extends SerialOSC {
  /* rocketOSFC class
  
    The rocketOSFC sub-classes the SerialOSC class, and implements the interface
    to an Open Source Flight Controller
    
    This class provides methods to send commands to the OSFC and Process OSC messages
    recieved from the OSFC.  Data recieved from the OSFC is stored in the object's
    properties, which then reflect the state of the OSFC.
    
  */
  
  int time, mode;
  boolean fet1_on, fet2_on, led_on, fet_go, launch_hold, chute_open, test_mode, arm_mode;
  boolean gps_valid, file_open, do_stage, do_deploy, do_main, do_record;
  int cal_time, launch_time, flight_time, stage_time, deploy1_time, deploy2_time, p_min, p_site, site_alt;
  int p, p_int, v_int, X_ACCEL, Y_ACCEL, Z_ACCEL, fet1_monitor, fet2_monitor;
  int p_deploy, p_launch, p_stage, pv_stage, h_deploy, h_launch, h_stage, v_stage;
  int meanAx, meanAy, meanAz, sqAx, sqAy, sqAz;
  int lat, lon, g_alt, p_alt, agl, alt_0;
  int pet_time, pet_timeout;
  boolean newSettings, newFlightData, newSensorData, newGPSdata; //newAccelData, 
  String modeString[] = {"Off", "Start", "Calibrate", "Launch Ready", "Launch Lock", "Flight", "Deployed", "Main Deploy", "Post Flight"};
  
  //rocketOSFC() {
  rocketOSFC(PApplet p) {
    super(p);
    time = 0;
    mode = 0;
    alt_0 = 0;
    lat = 35000069;
    lon = 106625080;
    g_alt = 5182;
    p_alt = 12;
    pet_timeout = 2000;
    gps_valid = false;
  }
  
  void processOSC() {
    //println("Got Serial Event");
    readString();
    //println("Read String");
    while (available()) {
      //println("String is OSC");
      petLink();
      SerialOSCmsg msg = read();
      println(msg);
      //displayText.scrollText(msg.toString());
      String m = msg.method;
      if (m.equals("Kk")) {
        
      } else if (m.equals("Nn")) {
        
      } else if (m.equals("ej")) {
        
        do_deploy = msg.getInt(0) > 0;
        
      } else if (m.equals("ma")) {
        
        do_main = msg.getInt(0) > 0;
        
      } else if (m.equals("sg")) {
        
        do_stage = msg.getInt(0) > 0;
        
      } else if (m.equals("te")) {
        
        test_mode = msg.getInt(0) > 0;
        
      } else if (m.equals("st")) {
        
        parseStatus(msg.getString(0));
        
      } else if (m.equals("da")) {
        
        parseSensor(msg.getString(0));
        newSensorData = true;
        
      /* } else if (m.equals("ac")) {
        
        parseAccel(msg.getString(0));
        newAccelData = true;
        */
     } else if (m.equals("fd")) {
        println(msg);
        parseFlight(msg.getString(0));
        newFlightData = true;
        
      } else if (m.equals("gp")) {
        
        parseGPS(msg.getString(0));
        newGPSdata = true;
        
      } else if (m.equals("se")) {
        
        parseSettings(msg.getString(0));
        newSettings = true;
      }
    }
  }
  
  String modeText() {
    return modeString[mode];
  }
  
  String FETstatus() {
    if (link_active()) {
      String s = "FET Hold";
      if (fet_go) s = "FET GO";
      return s;
    } else {
      return "---";
    }
  }
  
  color FETcolor() {
    color c = color(255, 0 , 0);
    if (fet_go && link_active()) c = color(0, 200, 0);
    return c;
  }
  
  String linkStatus() {
    String s = "Link Down";
    if (link_active()) s = "Link Active";
    return s;
  }
  
  color linkColor() {
    color c = color(255, 0, 0);
    if (link_active()) c = color(0, 200, 0);
    return c;
  }
  
  boolean link_active() {
    boolean b = false;
    if ((millis() - pet_time) < pet_timeout) b = true;
    return b;
  }
  
  void petLink() {
    pet_time = millis();
  }
  
  String latString() {
    return posString(lat);
  }
  
  String lonString() {
    return posString(lon);
  }
  
  String pAltString() {
    return altString(p_alt);
  }
  
  String gAltString() {
    if (gps_valid && g_alt < 50000) {
      return altString(g_alt);
    } else {
      return "---";
    }
  }
  
  String aglString() {
    return altString(agl);
  }
  
  // ***************************************************************
  // posString(x)
  // divides input x by 1E6, and returns a formatted String, with decimal point
  String posString(int x) {
    if (link_active() && gps_valid) {
      int n = x / 1000000;
      x -= n * 1000000;
      String s = str(x);
      String m = str(n);
      if (x < 10) s = "0" + s;
      if (x < 100) s = "0" + s;
      if (x < 1000) s = "0" + s;
      if (x < 10000) s = "0" + s;
      if (x < 100000) s = "0" + s;
      return m + "." + s;
    } else {
      return "---";
    }
  }
  
  String altString(int x) {
    if (link_active()) {
      int n = x / 1000;
      x -= n * 1000;
      String s = str(x);
      String m = "";
      if (n > 0) {
        m = str(n) + ",";
        if (x < 10) s = "0" + s;
        if (x < 100) s = "0" + s;
      }
      return m + s;
    } else {
      return "---";
    }
  }
  
  void parseGPS(String s) {
    String parts[] = split(s, ',');
    if (parts.length == 6) {
      lat = int(parts[1]);
      lon = int(parts[2]);
      g_alt = int(float(parts[3])/30.48);
      p_int = int(parts[5]);
      p_alt = p2alt(p_int) + alt_0;
      agl = p_alt - site_alt;
    }
  }
  
  void parseFlight(String s) {
    String parts[] = split(s, ',');
    if (parts.length == 10) {
      cal_time = int(parts[1]);
      launch_time = int(parts[2]);
      flight_time = int(parts[3]);
      stage_time = int(parts[4]);
      deploy1_time = int(parts[5]);
      deploy2_time = int(parts[6]);
      p_min = int(parts[7]);
      p_site = int(parts[8]);
      site_alt = int(parts[9]);
    }
  }
  
  void parseStatus(String s) {
    String parts[] = split(s, ',');
    if (parts.length == 5) {
      time = int(parts[1]);
      mode = int(parts[2]);
      int bits = int(parts[4]);
      fet1_on = (bits & 0x0001) != 0;
      fet2_on = (bits & 0x0802) != 0;
      led_on = (bits & 0x0004) != 0;
      fet_go = (bits & 0x0008) != 0;
      launch_hold = (bits & 0x0010) != 0;
      chute_open = (bits & 0x0020) != 0;
      test_mode = (bits & 0x0040) != 0;
      arm_mode = (bits & 0x0080) != 0;
      gps_valid = (bits & 0x0100) != 0;
      file_open = (bits & 0x0200) != 0;
      do_stage = (bits & 0x0400) != 0;
      do_deploy = (bits & 0x0800) != 0;
      do_main = (bits & 0x1000) != 0;
      do_record = (bits & 0x2000) != 0;
    }
    //println("Memory = " + parts[4]);
  }
  
  void parseSensor(String s) {
    String parts[] = split(s, ',');
    if (parts.length == 9) {
      p = int(parts[1]);
      p_int = int(parts[2]);
      v_int = int(parts[3]);
      X_ACCEL = int(parts[4]);
      Y_ACCEL = int(parts[5]);
      Z_ACCEL = int(parts[6]);
      fet1_monitor = int(parts[7]);
      fet2_monitor = int(parts[8]);
      p_alt = p2alt(p_int) + alt_0;
      agl = p_alt - site_alt;
    }
  }
  
  int p2alt(int pres) {
    return int(145439.6 * ( 1.0 - pow((float(pres) / 1024.0 / 101325.0), 0.18615)));
  }
  
  void parseSettings(String s) {
    String parts[] = split(s, ',');
    if (parts.length == 10) {
      p_deploy = int(parts[1]);
      p_launch = int(parts[2]);
      p_stage = int(parts[3]);
      pv_stage = int(parts[4]);
      h_deploy = int(parts[5]);
      h_launch = int(parts[6]);
      h_stage = int(parts[7]);
      v_stage = int(parts[8]);
      site_alt = int(parts[9]);
    }
  }
  /*
  void parseAccel(String s) {
    String parts[] = split(s, ',');
    if (parts.length == 10) {
      X_ACCEL = int(parts[1]);
      Y_ACCEL = int(parts[2]);
      Z_ACCEL = int(parts[3]);
      meanAx = int(parts[4]);
      meanAz = int(parts[6]);
      sqAx = int(parts[7]);
      sqAy = int(parts[8]);
      sqAz = int(parts[9]);
    } else {
      displayText.scrollText("Malformed Accel = " + s);
    }
  }
  */
  void setIntValue(String cmd, int value) {
    SerialOSCmsg msg = new SerialOSCmsg(cmd);
    msg.add(value);
    send(msg);
    msg = new SerialOSCmsg("p");
    send(msg);
  }
  
  public void setSiteAlt(int value) {
    setIntValue("e", value);
  }
  
  public void setMainAlt(int value) {
    setIntValue("a", value);
  }
  
  public void setGoAlt(int value) {
    setIntValue("G", value);
  }
  
  public void setStageAlt(int alt, int vel) {
    SerialOSCmsg msg = new SerialOSCmsg("s");
    msg.add(alt);
    send(msg);
    msg = new SerialOSCmsg("v");
    msg.add(vel);
    send(msg);
    msg = new SerialOSCmsg("p");
    send(msg);
  }
  
  public void refreshSettings() {
    //println("About to instantiate new msg");
    SerialOSCmsg msg = new SerialOSCmsg("p");
    //println("Sending msg:" + msg.toString());
    send(msg);
  }
  
  public void sendMain() {
    SerialOSCmsg msg = new SerialOSCmsg("m");
    send(msg);
  }
  
  public void sendDeploy() {
    SerialOSCmsg msg = new SerialOSCmsg("E");
    send(msg);
  }
  
  public void sendStage() {
    SerialOSCmsg msg = new SerialOSCmsg("S");
    send(msg);
  }
  
  public void toggleChuteLatch() { //_CODE_:chutePinCheck:786818:
    String s = "o";
    if (chute_open) s = "c";
    SerialOSCmsg msg = new SerialOSCmsg(s);
    send(msg);
  }
  
  public void setPreLaunch() {
    SerialOSCmsg msg = new SerialOSCmsg("L");
    send(msg);
  }
  
  public void sendHold() {
    SerialOSCmsg msg = new SerialOSCmsg("h");
    send(msg);
  }
  
  public void getSensorData() {
    SerialOSCmsg msg = new SerialOSCmsg("d");
    send(msg);
  }
  
  public void getAccelData() {
    SerialOSCmsg msg = new SerialOSCmsg("g");
    send(msg);
  }
  
  public void getFlightData() {
    SerialOSCmsg msg = new SerialOSCmsg("F");
    send(msg);
  }
  
  public void sendTest() {
    SerialOSCmsg msg = new SerialOSCmsg("t");
    send(msg);
  }
  
  public void toggleArm() {
    if (arm_mode) {
      SerialOSCmsg msg = new SerialOSCmsg("x");
      send(msg);
    } else {
      SerialOSCmsg msg = new SerialOSCmsg("A");
      send(msg);
    }
  }
  
  public void setFET(int fet) {
    SerialOSCmsg msg = new SerialOSCmsg();
    switch (fet) {
      case 1:
      {
        msg.method = "1";
      }
      case 2:
      {
        msg.method = "2";
      }
    }
    send(msg);
  }
  
  public void sendRecord() {
    SerialOSCmsg msg = new SerialOSCmsg("r");
    send(msg);
  }
  
  public void annotateFile(String s) {
    SerialOSCmsg msg = new SerialOSCmsg("r");
    msg.add(s);
    send(msg);
  }

}
